//
// Created by xuwj on 2021/10/25.
//
#include "TagItems.h"
#include "Converter.h"
#include "Eigen/Core"

namespace TagItems{


unsigned long PosePublisher::seq_ = 0;
unsigned long OdomSubscriber::seq_ = 0;


PosePublisher::PosePublisher(ros::NodeHandle &nh, string topic_name, int buff_size, string frame_id)
        : frame_id_(frame_id){
    publisher_ = nh.advertise<geometry_msgs::PoseStamped>(topic_name,buff_size);

}

void PosePublisher::publishPose(cv::Mat &mPose) {
    geometry_msgs::PoseStamped msg;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = frame_id_;
    msg.header.seq = seq_++;

    msg.pose.position.x = mPose.at<float>(0,3);
    msg.pose.position.y = mPose.at<float>(1,3);
    msg.pose.position.z = mPose.at<float>(2,3);
    cv::Mat rotation = mPose.rowRange(0,3).colRange(0,3);
    vector<float> q = VNavAssistant::Converter::toQuaternion(rotation);
    msg.pose.orientation.x = q[0];
    msg.pose.orientation.y = q[1];
    msg.pose.orientation.z = q[2];
    msg.pose.orientation.w = q[3];

    publisher_.publish(msg);
}

OdomSubscriber::OdomSubscriber(ros::NodeHandle &nh, string topic_name, int buff_size, string frame_id, int data_quantity)
        :frame_id_(frame_id),topic_name_(topic_name), data_quantity_limit(data_quantity){
    subscriber_ = nh.subscribe<nav_msgs::Odometry>(topic_name,buff_size,&OdomSubscriber::OdomMsgCallback,this);
}

void OdomSubscriber::ParseTagData(deque<TagOdomData> &tagOdom_data_buf) {
    if(!new_TagOdom_data_.empty()){
        tagOdom_data_buf.insert(tagOdom_data_buf.end(),new_TagOdom_data_.begin(),new_TagOdom_data_.end());
        new_TagOdom_data_.clear();
    }
}

void OdomSubscriber::OdomMsgCallback(const nav_msgs::Odometry::ConstPtr &odom_msg) {
    TagOdomData data;
    data.timestamp_ = odom_msg->header.stamp.toSec();

    geometry_msgs::Pose Tcam2map = odom_msg->pose.pose;
    Eigen::Quaterniond q(Tcam2map.orientation.w,Tcam2map.orientation.x,Tcam2map.orientation.y,Tcam2map.orientation.z);
    Eigen::Vector3d tcam2map(Tcam2map.position.x,Tcam2map.position.y,Tcam2map.position.z);
    Eigen::Matrix3d Rcam2map = q.toRotationMatrix();

    data.Tcam2map = VNavAssistant::Converter::toCvSE3(Rcam2map,tcam2map);
    new_TagOdom_data_.push_back(data);
    while(new_TagOdom_data_.size() > data_quantity_limit){
        new_TagOdom_data_.pop_front();
    }

}


}
